#include "PhysicsSPH.h"
#include <windows.h>

//=====================================================================

const float CPhysicalWorldSimulation::pi					  = 3.14159;
const float CPhysicalWorldSimulation::particle_radius		  = 0.004;
const float CPhysicalWorldSimulation::particle_diameter	  = particle_radius*2.0;//1.0e-06;
const float CPhysicalWorldSimulation::support_radius		  = 0.01;//particle_radius*4.0;//2.0e-06;
const float CPhysicalWorldSimulation::support_radius_sqr	  = support_radius*support_radius;
const float CPhysicalWorldSimulation::sr3					  = support_radius*support_radius*support_radius;
const float CPhysicalWorldSimulation::sr6					  = sr3*sr3;
const float CPhysicalWorldSimulation::sr9					  = sr3*sr6;
const float CPhysicalWorldSimulation::c315div64pi_h9		  = 315.f/(64.f*pi*sr9);
//const float CPhysicalWorldSimulation::c945div32pi_h9		  = 945.f/(32.f*pi*sr9);
const float CPhysicalWorldSimulation::c45div_pi_h6			  = 45.f/(     pi*sr6);
const float CPhysicalWorldSimulation::waterViscosityConstant  = 0.1;//1.0e-3;// Pa*s
//const float CPhysicalWorldSimulation::delta_t				  = 10000.0*0.4*support_radius/1500.0;//0.4 - alpha, 1500.0 - speed of sound in liquid
const float CPhysicalWorldSimulation::delta_t				  = 0.003;//1*0.4*support_radius/1500.0;//0.4 - alpha, 1500.0 - speed of sound in liquid
const float CPhysicalWorldSimulation::delta_t_sqr			  = delta_t*delta_t;
const float CPhysicalWorldSimulation::delta_t_sqr_d2		  = delta_t_sqr/2.0;
const float CPhysicalWorldSimulation::particle_mass			  = 0.00020543;//1.01546e-15;// kg
const float CPhysicalWorldSimulation::rho_0					  = 600.0;// kg/m^3
//const float CPhysicalWorldSimulation::rho_0				  = (6.0*W_poly6(1.0*particle_diameter)+6.0*W_poly6(2.0*particle_diameter)+
//																12.0*W_poly6(sqrt(2.0)*particle_diameter)+8.0*W_poly6(sqrt(3.0)*particle_diameter)/**/+W_poly6(0.0)/**/) * particle_mass;
//const double CPhysicalWorldSimulation::sph_beta				  = 2.0*pow(particle_mass*delta_t/rho_0,2);
//const double CPhysicalWorldSimulation::sph_delta			  = calcDelta();

//=====================================================================

void CPhysicalWorldSimulation::init3Dgrid()
{
	cell.clear();

	cell.resize(cells_x);

	for(int x = 0; x<cells_x; x++)
	{
		cell[x].resize(cells_y);

		for(int y = 0; y<cells_y; y++)
		{
			cell[x][y].resize(cells_z);
		}
	}

	return;
}

void CPhysicalWorldSimulation::findNeighbours()
{
	int index = 0;

	int i,i2;
	int x,y,z;// particle coordinates in grid
	int x2,y2,z2;
	Vector3D pos,pos2;
	float h_sq = support_radius*support_radius;
	// h*h, h - neighborhood sphere radius which encloses 30-40 particles at average
	float r_sq, x_sq, y_sq, z_sq;
	int particle_counter = 0;
	double neighbor_counter = 0;

	for(x=0;x<cells_x;x++)
	{
		for(y=0;y<cells_y;y++)
		{
			for(z=0;z<cells_z;z++)
			{
				cell[x][y][z].particle_index.clear();
			}
		}
	}

	for(i=0;i<(int)particle.size();i++)
	{
		particle[i].neighborIndex.clear();
		particle[i].neighborIndex.reserve(50);
		particle[i].NNdist = -1;
		
		pos = particle[i].getPos();


		x = (int)(0.5f*pos.x/particle_radius);
		y = (int)(0.5f*pos.y/particle_radius);
		z = (int)(0.5f*pos.z/particle_radius);

		if( (x>=0)&&(x<cells_x)&&
			(y>=0)&&(y<cells_y)&&
			(z>=0)&&(z<cells_z) )
		{
			cell[x][y][z].particle_index.push_back(i);
			particle_counter++;
		}
		//else //Alert!!! Particle out of world boundaries
	}

	/*
	DWORD dwEnd = GetTickCount();
	init_time = dwEnd - dwStart;
	return;
	/**/

	int p_sz = 0;

	for(x=0;x<cells_x;x++)
	{
		for(y=0;y<cells_y;y++)
		{
			for(z=0;z<cells_z;z++)
			{
				p_sz += cell[x][y][z].particle_index.size();

				for(i=0;i<(int)cell[x][y][z].particle_index.size();i++)
				{
					neighbor_counter = 0;
					pos = particle[ cell[x][y][z].particle_index[i] ].pos;
					//particle[ cell[x][y][z].particle_index[i] ].neighborIndex.clear();
					//particle[ cell[x][y][z].particle_index[i] ].neighborIndex.reserve(50);
					//NNdist

					/////////////////////////////////////////////
					for(x2=max(0,x-2);x2<=min(cells_x-1,x+2);x2++)
					{
						for(y2=max(0,y-2);y2<=min(cells_y-1,y+2);y2++)
						{
							for(z2=max(0,z-2);z2<=min(cells_z-1,z+2);z2++)
							{
								for(i2=0;i2<(int)cell[x2][y2][z2].particle_index.size();i2++)
								{
									if(cell[x][y][z].particle_index[i]!=cell[x2][y2][z2].particle_index[i2])
									{
										pos2 = particle[ cell[x2][y2][z2].particle_index[i2] ].pos;
										
										x_sq = (float)abs(pos.x - pos2.x); 
										y_sq = (float)abs(pos.y - pos2.y); 
										z_sq = (float)abs(pos.z - pos2.z); 

										//if( (x_sq<= h_sq+1e-20) && (y_sq<= h_sq+1e-20) && (z_sq<= h_sq+1e-20) )
										{
											x_sq *= x_sq;
											y_sq *= y_sq;
											z_sq *= z_sq;

											r_sq = (float)(x_sq+y_sq+z_sq);

											if(r_sq <= h_sq) 
											{
												particle[ cell[x][y][z].particle_index[i] ].neighborIndex.push_back( cell[x2][y2][z2].particle_index[i2] );
												neighbor_counter++;
												if(neighbor_counter==1) particle[ cell[x][y][z].particle_index[i] ].NNdist = r_sq;
												else particle[ cell[x][y][z].particle_index[i] ].NNdist = min(particle[ cell[x][y][z].particle_index[i] ].NNdist,r_sq);
											}
										}
									}
								}
							}
						}
					}
					/////////////////////////////////////////////

					if(neighbor_counter>=1)
					{
						double nnd = particle[ cell[x][y][z].particle_index[i] ].NNdist;
						nnd = particle[ cell[x][y][z].particle_index[i] ].NNdist = sqrt(particle[ cell[x][y][z].particle_index[i] ].NNdist)/particle_diameter;
						r_sq = r_sq;
					}
				}
			}
		}
	}


	return;
}

CPhysicalWorldSimulation::CPhysicalWorldSimulation(int cells_x, int cells_y, int cells_z)
{

	initializedSuccessfully = 0;

	this->cells_x = cells_x;
	this->cells_y = cells_y;
	this->cells_z = cells_z;

	if( (cells_x<=0) || (cells_y<=0) || (cells_z<=0) ) initializedSuccessfully = 0; else initializedSuccessfully = 1;

	if(initializedSuccessfully) 
	{
		//initWorldConstants();

		DWORD dwStart = GetTickCount();
		/**/prepareDamBreakDemo();
		/**/init3Dgrid();
		DWORD dwEnd = GetTickCount();
		init_time = dwEnd - dwStart;

		dwStart = GetTickCount();
		/**/findNeighbours();
		dwEnd = GetTickCount();
		init_time2 = dwEnd - dwStart;
	}


	return;
}

void CPhysicalWorldSimulation::prepareDamBreakDemo()
{
	/*
	int wx,wy,wz;
	wx = (cells_x*support_radius/particle_radius)/2;
	wy = (cells_y*support_radius/particle_radius)/2;// half of cube in each direction
	wz = (cells_z*support_radius/particle_radius)/2;
	*/

	float dx,dy,dz;
	int estimated_size = (cells_x*9/10)*(cells_y*9/10)*(cells_z*9/10)+100;

	particle.reserve(estimated_size);
/**/
	//for(double x=3*particle_radius;x<cells_x*(particle_radius*2)*3/20;x+=particle_radius*2/2)
	for(double x=cells_x*(particle_radius*2)*2/9;x<cells_x*(particle_radius*2)*3/9;x+=particle_radius*1.1)
	{
		for(double y=3*particle_radius;y<cells_y*(particle_radius*2)*19/20;y+=particle_radius*1.1)
		{
			for(double z=3*particle_radius;z<cells_z*(particle_radius*2)*19/20;z+=particle_radius*2/2)
			{
				dx = particle_radius*float( rand() * 0.05 ) / RAND_MAX;
				dy = particle_radius*float( rand() * 0.05 ) / RAND_MAX;
				dz = particle_radius*float( rand() * 0.05 ) / RAND_MAX;
				particle.push_back(CParticle(x+dx,y+dy,z+dz,(float)particle_mass));
				particle[particle.size()-1].vel = Vector3D(0,0,0);
				particle[particle.size()-1].viscosity = (float)waterViscosityConstant;
				particle[particle.size()-1]._isBoundary = 0;
			}
		}
	}

	return;
/**/
	// boundaries
	int status = 0;

	for(double x=particle_radius,status=0;x<cells_x*(particle_radius*2);x+=(cells_x-1)*(particle_radius*2),status++)
	{
		for(double y=particle_radius;y<cells_y*(particle_radius*2);y+=particle_radius*2)
		{
			for(double z=particle_radius;z<cells_z*(particle_radius*2);z+=particle_radius*2)
			{
				particle.push_back(CParticle((float)x,(float)y,(float)z,(float)particle_mass));
				particle[particle.size()-1].vel = Vector3D(0,0,0);
				particle[particle.size()-1].viscosity = (float)waterViscosityConstant;
				particle[particle.size()-1]._isBoundary = 1;
				particle[particle.size()-1].normal = Vector3D(1.0-status*2.0,0,0);

				if(y<2*particle_radius) { particle[particle.size()-1].normal = Vector3D(1.0-status*2.0,1.f,0.f); particle[particle.size()-1].normal.unitize(); }
				if(y>2*particle_radius*(cells_y-1)) { particle[particle.size()-1].normal = Vector3D(1.0-status*2.0,-1.f,0.f); particle[particle.size()-1].normal.unitize(); }

				if(z<2*particle_radius) { particle[particle.size()-1].normal = Vector3D(1.0-status*2.0,0.f,1.f); particle[particle.size()-1].normal.unitize(); }
				if(z>2*particle_radius*(cells_z-1)) { particle[particle.size()-1].normal = Vector3D(1.0-status*2.0,0.f,-1.f); particle[particle.size()-1].normal.unitize(); }

				if((y<2*particle_radius)&&(z<2*particle_radius)) 
					{ particle[particle.size()-1].normal = Vector3D(1.0-status*2.0, 1.f, 1.f); particle[particle.size()-1].normal.unitize(); }
				
				if((y>2*particle_radius*(cells_y-1))&&(z<2*particle_radius)) 
					{ particle[particle.size()-1].normal = Vector3D(1.0-status*2.0,-1.f, 1.f); particle[particle.size()-1].normal.unitize(); }

				if((y<2*particle_radius)&&(z>2*particle_radius*(cells_z-1))) 
					{ particle[particle.size()-1].normal = Vector3D(1.0-status*2.0, 1.f,-1.f); particle[particle.size()-1].normal.unitize(); }

				if((y>2*particle_radius*(cells_y-1))&&(z>2*particle_radius*(cells_z-1))) 
					{ particle[particle.size()-1].normal = Vector3D(1.0-status*2.0,-1.f,-1.f); particle[particle.size()-1].normal.unitize(); }
			}
		}
	}

	for(double x=particle_radius*3;x<(cells_x-1)*(particle_radius*2);x+=particle_radius*2)
	{
		for(double y=particle_radius,status=0;y<cells_y*(particle_radius*2);y+=(cells_y-1)*(particle_radius*2),status++)
		{
			for(double z=particle_radius;z<cells_z*(particle_radius*2);z+=particle_radius*2)
			{
				particle.push_back(CParticle((float)x,(float)y,(float)z,(float)particle_mass));
				particle[particle.size()-1].vel = Vector3D(0,0,0);
				particle[particle.size()-1].viscosity = (float)waterViscosityConstant;
				particle[particle.size()-1]._isBoundary = 1;
				particle[particle.size()-1].normal = Vector3D(0,1.0-status*2.0,0);

				if(x<2*particle_radius) { particle[particle.size()-1].normal = Vector3D(1.f,1.0-status*2.0,0.f); particle[particle.size()-1].normal.unitize(); }
				if(x>2*particle_radius*(cells_x-1)) { particle[particle.size()-1].normal = Vector3D(-1.f,1.0-status*2.0,0.f); particle[particle.size()-1].normal.unitize(); }

				if(z<2*particle_radius) { particle[particle.size()-1].normal = Vector3D(0.f,1.0-status*2.0,1.f); particle[particle.size()-1].normal.unitize(); }
				if(z>2*particle_radius*(cells_z-1)) { particle[particle.size()-1].normal = Vector3D(0.f,1.0-status*2.0,-1.f); particle[particle.size()-1].normal.unitize(); }
			}
		}
	}

	for(double x=particle_radius*3;x<(cells_x-1)*(particle_radius*2);x+=particle_radius*2)
	{
		for(double y=particle_radius*3;y<(cells_y-1)*(particle_radius*2);y+=particle_radius*2)
		{
			for(double z=particle_radius,status=0;z<cells_z*(particle_radius*2);z+=(cells_z-1)*(particle_radius*2),status++)
			{
				particle.push_back(CParticle((float)x,(float)y,(float)z,(float)particle_mass));
				particle[particle.size()-1].vel = Vector3D(0,0,0);
				particle[particle.size()-1].vel_eval = Vector3D(0,0,0);
				particle[particle.size()-1].viscosity = (float)waterViscosityConstant;
				particle[particle.size()-1]._isBoundary = 1;
				particle[particle.size()-1].normal = Vector3D(0,0,1.0-status*2.0);
			}
		}
	}

	return;
}

int CPhysicalWorldSimulation::getNumberOfParticles()
{
	return particle.size();
}

float CPhysicalWorldSimulation::getParticleRadius()
{
	return particle_radius;
}

Vector3D CPhysicalWorldSimulation::getCellsXYZ()
{
	return Vector3D(cells_x,cells_y,cells_z);
}

CParticle * CPhysicalWorldSimulation::getParticle(int i)
{
	return &particle[i];
}


//////////////////////////////////////
/*
void CPhysicalWorldSimulation::initWorldConstants()
{

}
*/
/*
double CPhysicalWorldSimulation::calcDelta()
{
	float x[32] = { 1, 1, 0,-1,-1,-1, 0, 1, 1, 1, 0,-1,-1,-1, 0, 1, 1, 1, 0,-1,-1,-1, 0, 1, 2,-2, 0, 0, 0, 0, 0, 0};
	float y[32] = { 0, 1, 1, 1, 0,-1,-1,-1, 0, 1, 1, 1, 0,-1,-1,-1, 0, 1, 1, 1, 0,-1,-1,-1, 0, 0, 2,-2, 0, 0, 0, 0};
	float z[32] = { 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1,-1,-1,-1,-1,-1,-1,-1,-1, 0, 0, 0, 0, 2,-2, 1,-1};

	Vector3D GradWij0;
	Vector3D SumGradWij0 = Vector3D(0,0,0);
	double SumGradWij0_GradWij0 = 0;

	for(int i=0;i<32;i++)
	{
		//GradWij0 = grad_W_poly6(Vector3D(x[i],y[i],z[i])*particle_diameter);
		GradWij0 = grad_W_spiky(Vector3D(x[i],y[i],z[i])*particle_diameter);
		SumGradWij0 += GradWij0;
		SumGradWij0_GradWij0 += GradWij0*GradWij0;
	}

	return 1.0/(sph_beta*(SumGradWij0*SumGradWij0 + SumGradWij0_GradWij0));// delta;
	//return 1.0;
}
*/

/*
double CPhysicalWorldSimulation::W_poly6(double distance)
{
	return c315div64pi_h9 * pow(support_radius*support_radius - distance*distance, 3);
}
*/
/*
Vector3D CPhysicalWorldSimulation::grad_W_poly6(Vector3D v_ij)
{
	double r_sqr = v_ij.getLengthSq_fast();

	if(r_sqr>support_radius_sqr) return Vector3D(0,0,0);
	else
	return v_ij*(-c945div32pi_h9*pow(support_radius_sqr - r_sqr,2));
}
*/
/*
Vector3D CPhysicalWorldSimulation::grad_W_spiky(Vector3D v_ij)
{
	double r_sqr = v_ij.getLengthSq_fast();

	if(r_sqr>support_radius_sqr) 
	{
		return Vector3D(0,0,0);
	}
	else
	return v_ij.unit()*(-c45div_pi_h6*pow(support_radius - sqrt(r_sqr),2));
}
*/
/*
float Laplace_W_poly6(float distance)
{
	float r_sqr = distance*distance;

	return -c945div32pi_h9*(support_radius_sqr - r_sqr)*(support_radius_sqr*3 - r_sqr*7);
}
*/

//////////////////////////////////////////////////////////////////////////////////////////
//	W_poly6
//////////////////////////////////////////////////////////////////////////////////////////
/*
double CPhysicalWorldSimulation::W_poly6(int i, int j, int index)
{
	Vector3D v_ij = particle[i].getPos(index) - particle[j].getPos(index);
	double r_sqr = v_ij.getLengthSq_fast();

	if(r_sqr>support_radius_sqr) return 0;
	else
	return c315div64pi_h9 * pow(support_radius_sqr - r_sqr, 3);
	//return c315div64pi_h9 * pow(max(0,support_radius*support_radius - v_ij.getLengthSq_fast()), 3);
}
*/
/*
Vector3D CPhysicalWorldSimulation::grad_W_poly6(int i, int j, int index)
{
	if(!((index==0)||(index==1))) return Vector3D(0,0,0);

	Vector3D v_ij = particle[i].getPos(index) - particle[j].getPos(index);
	double r_sqr = v_ij.getLengthSq_fast();

	if(r_sqr>support_radius_sqr) return Vector3D(0,0,0);
	else
	return v_ij*(-c945div32pi_h9*pow(support_radius_sqr - r_sqr,2));
}
*/
/*
double CPhysicalWorldSimulation::Laplace_W_poly6(int i, int j, int index)
{
	if(!((index==0)||(index==1))) return 0;

	Vector3D v_ij = particle[i].getPos(index) - particle[j].getPos(index);
	double r_sqr = v_ij.getLengthSq_fast();

	if(r_sqr>support_radius_sqr) return 0;
	else
	return -c945div32pi_h9*(support_radius_sqr - r_sqr)*(support_radius_sqr*3 - r_sqr*7);
}
*/
//////////////////////////////////////////////////////////////////////////////////////////
//	W_spiky
//////////////////////////////////////////////////////////////////////////////////////////

/*
Vector3D CPhysicalWorldSimulation::grad_W_spiky(int i, int j, int index)
{
	if(!((index==0)||(index==1))) return Vector3D(0,0,0);

	Vector3D v_ij = particle[i].getPos(index) - particle[j].getPos(index);
	double r_sqr = v_ij.getLengthSq_fast();
	double r = sqrt(r_sqr);


	if(r>support_radius)
	{
		return Vector3D(0,0,0);
	}
	else
	return v_ij.unit()*(-1.0*c45div_pi_h6*(support_radius - r)*(support_radius - r));//pow(support_radius - r,2));
}
*/

//////////////////////////////////////////////////////////////////////////////////////////
//	W_viscous
//////////////////////////////////////////////////////////////////////////////////////////
/*
double CPhysicalWorldSimulation::Laplace_W_viscous(int i, int j, int index)
{
	if(!((index==0)||(index==1))) return 0.f;

	Vector3D v_ij = particle[i].getPos(index) - particle[j].getPos(index);
	double r_sqr = v_ij.getLengthSq_fast();

	return c45div_pi_h6*(support_radius - sqrt(r_sqr));
}
*/

//////////////////////////////////////////////////////////////////////////////////////////
/*
void CPhysicalWorldSimulation::computeDensity()
{
	if(!((index==0)||(index==1))) return;

	int j;

	for(int i=0;i<(int)particle.size();i++)
	{
		particle[i].density[index] = 0.f;

		for(int k=0;k<(int)particle[i].neighborIndex.size();k++)
		{
			j = particle[i].neighborIndex[k];
 			particle[i].density += (particle[j].mass * W_poly6(i,j,index));
		}

		particle[i].density += (particle[i].mass * W_poly6(i,i,index));

		//if(particle[i].density[index]<0.01) particle[i].density[index] = 0.01;
	}
}

void CPhysicalWorldSimulation::computeViscosityForce(int index)
{
	if(!((index==0)||(index==1))) return;

	int j;
	Vector3D viscosityForce_i;
 
	for(int i=0;i<(int)particle.size();i++)
	{
		particle[i].viscosityForce = Vector3D(0.f,0.f,0.f);
		viscosityForce_i = Vector3D(0.f,0.f,0.f);

		for(int k=0;k<(int)particle[i].neighborIndex.size();k++)
		{
			j = particle[i].neighborIndex[k];
			viscosityForce_i
				+= (particle[j].vel[0] - particle[i].vel[0])*(particle[i].viscosity+particle[j].viscosity)*0.5f
				*  (particle[j].mass/particle[j].density[0])*Laplace_W_viscous(i,j,index);
		}

		particle[i].viscosityForce += viscosityForce_i * 1 * particle[i].mass / particle[i].density[0];
	}
}

void CPhysicalWorldSimulation::computeGravityForce(int index)
{
	if(!((index==0)||(index==1))) return;

	for(int i=0;i<(int)particle.size();i++)
	{
		particle[i].gravityForce +=  Vector3D(0,0,-9.81)*particle[i].mass;
	}
}
*/

void CPhysicalWorldSimulation::AdvanceInTime()
{
	unsigned int i;

	unsigned int q,j;
/*
	double w_c_ib;
	double sum_w_c_ib;
	Vector3D n_c_i;
	Vector3D x_i_corr;
*/

/*
	for(i=0;i<particle.size();i++)
	{
		int j;
		int b_cnt = 0;
		sum_w_c_ib = 0;
		w_c_ib = 0;
		n_c_i = Vector3D(0,0,0);
		x_i_corr = Vector3D(0,0,0);

		if(!particle[i].isBoundary())
		{
			for(q=0;q<particle[i].neighborIndex.size();q++)
			{
				j = particle[i].neighborIndex[q];

				if(particle[j].isBoundary())
				{
					w_c_ib = max(0,1.0 - 2.0*(particle[i].getPos(0) - particle[j].getPos(0)).length()/support_radius);
					if(w_c_ib>0) b_cnt++;
				}

				n_c_i += particle[j].getNormal()*w_c_ib;
				sum_w_c_ib += w_c_ib;
			}

			if(b_cnt)
			{
				for(q=0;q<particle[i].neighborIndex.size();q++)
				{
					j = particle[i].neighborIndex[q];

					if(particle[j].isBoundary())
					{
						w_c_ib = max(0,1.0 - 2.0*(particle[i].getPos(0) - particle[j].getPos(0)).length()/support_radius);
						x_i_corr += n_c_i.unit()*w_c_ib*(0.5*support_radius - (particle[i].getPos(0) - particle[j].getPos(0)).length());
					}
				}

				particle[i].pos[1] += x_i_corr/sum_w_c_ib;
				double vel_n_len = n_c_i*particle[i].vel[0];
				if(vel_n_len<0)
				{
					Vector3D vel_n = n_c_i.unit()*vel_n_len;
					particle[i].vel[0] += -vel_n;
				}
				//particle[i].vel[1] *= 0.99;//epsilon
			}
		}
	}
*/
	findNeighbours();

	float sum;
	float dx,dy,dz,dr_sqr,dr;


	for(i=0;i<particle.size();i++)
	{
		sum = 0;

		for(int q=0;q<(int)particle[i].neighborIndex.size();q++)
		{
			j = particle[i].neighborIndex[q];
			dr_sqr = (particle[i].pos - particle[j].pos).getLengthSq_fast();
			if(support_radius_sqr>dr_sqr)
			{
				sum += pow(support_radius_sqr - dr_sqr,3);
			}	
			else
			{
				continue;
			}
		}

		particle[i].density = sum * particle[i].mass * c315div64pi_h9;
		//p->density = sum * m_Param[SPH_PMASS] * m_Poly6Kern ;	
		particle[i].pressure = (particle[i].density - rho_0)*0.5/*stiffness*/;
		//particle[i].pressure = (pow(particle[i].density/rho_0,3) - 1)*rho_0*0.5/3/*stiffness*/;
		//p->pressure = ( p->density - m_Param[SPH_RESTDENSITY] ) * m_Param[SPH_INTSTIFF];		
		//p->density = 1.0f / p->density;	
	}

	float hmr;//h(supp.rad.) - r_ij
	float pterm,dterm,vterm;

	// compute pressure force
	/*
	c = ( mR - m_NDist[i][j] );
	pterm = -0.5f * c * m_SpikyKern * ( p->pressure + pcurr->pressure) / m_NDist[i][j];
	dterm = c * p->density * pcurr->density;
	vterm = m_LapKern * visc;
	force.x += ( pterm * dx + vterm * (pcurr->vel_eval.x - p->vel_eval.x) ) * dterm;
	force.y += ( pterm * dy + vterm * (pcurr->vel_eval.y - p->vel_eval.y) ) * dterm;
	force.z += ( pterm * dz + vterm * (pcurr->vel_eval.z - p->vel_eval.z) ) * dterm;
	*/

	for(i=0;i<particle.size();i++) // both for water and boundary points
	{
		if(!particle[i].isBoundary())
		{
			particle[i].pressureForce = Vector3D(0,0,0);
			particle[i].viscosityForce = Vector3D(0,0,0);
			particle[i].gravityForce =  Vector3D(0,0,-9.81);

			for(int q=0;q<(int)particle[i].neighborIndex.size();q++)
			{
				j = particle[i].neighborIndex[q];
				dr_sqr = (particle[i].pos - particle[j].pos).getLengthSq_fast();
				dr = sqrt(dr_sqr);
				hmr = support_radius - dr;
				pterm = -0.5f*hmr*(-c45div_pi_h6)*(particle[i].pressure+particle[j].pressure)/dr;
				dterm = hmr / (particle[i].density * particle[j].density);
				vterm = c45div_pi_h6 * waterViscosityConstant;

				particle[i].pressureForce  += (particle[i].pos - particle[j].pos)*pterm*dterm;
				//Vector3D vpf = particle[i].pressureForce;
				particle[i].viscosityForce += (particle[j].vel_eval - particle[i].vel_eval)*vterm*dterm;
			}
		}
	}

	/*
		// Compute Acceleration		
		accel = p->sph_force;
		accel *= m_Param[SPH_PMASS];

		// Velocity limiting 
		speed = accel.x*accel.x + accel.y*accel.y + accel.z*accel.z;
		if ( speed > SL2 ) {
			accel *= SL / sqrt(speed);
		}	
	*/

	float epsilon = 1e-5;
	float stiff = 20000.0;
	float damp = 256.0;
	float diff;
	Vector3D norm;
	float adj;

	float speed;
	Vector3D accel,vnext;
	float SL = 200;

	for(i=0;i<particle.size();i++) 
	{
		accel = particle[i].pressureForce + particle[i].viscosityForce;
		accel *= particle_mass;
		accel += particle[i].gravityForce;

		speed = accel.x*accel.x + accel.y*accel.y + accel.z*accel.z;
		if ( speed > SL*SL ) accel *= SL / sqrt(speed);

	// Boundary Conditions
	// Z-axis walls
	/*
	float eps = EPSILON;
	diff = 2 * radius - ( max.z - p->pos.z )*ss;
	if (diff > EPSILON) {
		norm.Set ( 0, 0, -1 );
		adj = stiff * diff - damp * norm.Dot ( p->vel_eval );
		accel.x += adj * norm.x; accel.y += adj * norm.y; accel.z += adj * norm.z;
	}
	*/

		// z-axis
		diff = 2*particle_radius - particle[i].pos.z;
		if(diff>epsilon)
		{
			norm = Vector3D(0,0,1);
			adj = stiff*diff - damp*(norm*particle[i].vel_eval);
			accel += norm*adj;
		}

		diff = 2*particle_radius - (2*particle_radius*cells_z - particle[i].pos.z);
		if(diff>epsilon)
		{
			norm = Vector3D(0,0,-1);
			adj = stiff*diff - damp*(norm*particle[i].vel_eval);
			accel += norm*adj;
		}

		// y-axis
		diff = 2*particle_radius - particle[i].pos.y;
		if(diff>epsilon)
		{
			norm = Vector3D(0,1,0);
			adj = stiff*diff - damp*(norm*particle[i].vel_eval);
			accel += norm*adj;
		}

		diff = 2*particle_radius - (2*particle_radius*cells_y - particle[i].pos.y);
		if(diff>epsilon)
		{
			norm = Vector3D(0,-1,0);
			adj = stiff*diff - damp*(norm*particle[i].vel_eval);
			accel += norm*adj;
		}

		// x-axis
		diff = 2*particle_radius - particle[i].pos.x;
		if(diff>epsilon)
		{
			norm = Vector3D(1,0,0);
			adj = stiff*diff - damp*(norm*particle[i].vel_eval);
			accel += norm*adj;
		}

		diff = 2*particle_radius - (2*particle_radius*cells_x - particle[i].pos.x);
		if(diff>epsilon)
		{
			norm = Vector3D(-1,0,0);
			adj = stiff*diff - damp*(norm*particle[i].vel_eval);
			accel += norm*adj;
		}



		vnext = accel * delta_t + particle[i].vel;
		particle[i].vel_eval = particle[i].vel;
		particle[i].vel_eval += vnext;
		particle[i].vel_eval *= 0.5;
		particle[i].vel = vnext;
		particle[i].pos += vnext*delta_t;

	}



/*
	// Leapfrog Integration ----------------------------
		vnext = accel;							
		vnext *= m_DT;
		vnext += p->vel;						// v(t+1/2) = v(t-1/2) + a(t) dt
		p->vel_eval = p->vel;
		p->vel_eval += vnext;
		p->vel_eval *= 0.5;					// v(t+1) = [v(t-1/2) + v(t+1/2)] * 0.5		used to compute forces later
		p->vel = vnext;
		vnext *= m_DT/ss;
		p->pos += vnext;						// p(t+1) = p(t) + v(t+1/2) dt
*/

}



//////////////////////////////////////
//
//		CParticle
//
//////////////////////////////////////

CParticle::CParticle(float x, float y, float z, float particle_mass)
{
	pos = Vector3D(x,y,z);
	vel = Vector3D(0,0,0);
	mass = particle_mass;
}

Vector3D CParticle::getPos()
{
	//if(!((index==0)||(index==1))) return Vector3D(0,0,0);

	return pos;//[index];
}
/*
void CParticle::SetPos(float x, float y, float z)
{
	pos[0].x = x;
	pos[0].y = y;
	pos[0].z = z;
}

void CParticle::SetPos(Vector3D v)
{
	pos[0] = v;
}
*/
vector <int> * CParticle::getNeighborIndex()
{
	return &neighborIndex;
}

bool CParticle::isBoundary()
{
	return _isBoundary;
}

Vector3D CParticle::getNormal()
{
	return normal;
}

float CParticle::getDensity()
{
	return density;
}

float CParticle::getNNdist()
{
	return NNdist;
}